 /** @file base.c
 * @brief Reads control values from the joystick and transmits them to Blimp.
 *
 *  NB!: Must be compiled with optimisation at least -O2
 *
 *    @author Aaron Kaspar
 *    @author Colburn Holbrook
 *	  @author Josh Fairhead
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include <string.h>

#include "radio.h"
#include "../uart.h"
#include "../common.h"
#include "joystick.h"


#define LED_D5_RED        0x80

/** The buffer to hold the packet contents. */
volatile uint8_t radio_buf[PAYLOAD_BYTES];
/** The flag for a radio packet has been received (1) and is ready to be processed. */
volatile uint8_t packet_available;
/** The period of time to delay in main loop. */
#define POLL_INTERVAL 25
/** The number of times to complete the loop before timeout of receive mode. */
#define TIME_OUT 1000/POLL_INTERVAL

uint8_t cState;

/* define type for current state */
typedef enum
{
   INIT=1,
   CONTROLLER=2,
   LISTEN=3
} current_state;

/**
 *  @brief initialize all components
 *
 */
void init (){

	//set Clock on and enable all interrupts
	CLKPR = 0x80;
	CLKPR = 0x00;

    uart_init();

    // Init an LED on the key
    PORTD ^= LED_D5_RED;
    DDRD |= LED_D5_RED;


	/* Initialize Radio */
   radio_init((uint8_t) RADIO_CHANNEL, BASE_ADDRESS, RX_MODE);
   packet_available = 0;
	cState = INIT;


    sei();


}

/**
 *  @brief Control loop for reading values from the joystick and sending to the blimp
 *
 * This code waits indefinately for a connection from the blimp.  Once connection is
 * established, it samples the joystick at a regular rate and sends the values to the blimp
 * After sending a control packet, it waits until a status packet is returned from the 
 * blimp.
 */
int main(int argc, char**argv)
{

	init();

	char buffer[50];
	unsigned char X = 0, Y = 0, Z = 0;


	packet p, *q;		// p is sending, *q is receiving
 	int count=0;

   /* Initialize "data" being sent to blimp */
   	sample_joystick(&X, &Y, &Z);
   p.payload.control.X=(uint8_t)X;
   p.payload.control.Y=(uint8_t)Y;
   p.payload.control.Z=(uint8_t)Z;

   /* Packet structure must = PAYLOAD_BYTES otherwise radio will not work propertly */
   if (sizeof(packet) != PAYLOAD_BYTES)
   {
      uart_println( "Incorrect packet size (%d)!\n", sizeof(packet) );
      return 0;
   }


	sprintf(buffer, "\r\n************* Wilkommen in Basestation **************\r\n");
	uart_write((uint8_t*)buffer, strlen(buffer));

int i;

    for(;;){


      switch(cState)
      {
         case(INIT):

			sprintf(buffer, "Init cState: %u\r\n",cState);
			uart_write((uint8_t*)buffer, strlen(buffer));
            /* Wait for ready message from blimp */

			while(!packet_available);

            q = ((packet*) radio_buf);		// q points to radio_buf which is in packet format
            switch(q->type)
            {
               case READY:

                  uart_println("\t%s\r\n",q->payload.msg);
			      cState=LISTEN;

				  	sprintf(buffer, "READY msg received, cState=%u\r\n", cState);
					uart_write((uint8_t*)buffer, strlen(buffer));

				  radio_set_receive();
			      count=0;
                  break;
               case STATUS:
               case CONTROL:
               case DEBUG:
               default:
                  uart_println("\tBlimp not ready!\r\n");
                  return(0);
            }
            packet_available = 0;
            break;
         case(LISTEN):
            if (packet_available)
            {
               q = ((packet*) radio_buf);		// q points to radio_buf which is in packet format
               switch(q->type)
               {
                  case STATUS:

                     uart_println("\tCurrent Height: %u\r\n",q->payload.status.currHeight);
                     cState=CONTROLLER;

					sprintf(buffer, "BLIMP: currHeight:%d, setHeight:%d, heightDiff:%d.\r\n", q->payload.status.currHeight, q->payload.status.setHeight, q->payload.status.heightDiff);
					uart_write((uint8_t*)buffer, strlen(buffer));

					 // set_standby_mode() is set at end of ISR
                     break;
                  case DEBUG:
                     uart_println("\t%s\r\n",q->payload.msg);
                     break;
                  case READY:
				  	sprintf(buffer, "In LISTEN ,READY msg received, cState=%u\r\n", cState);
					uart_write((uint8_t*)buffer, strlen(buffer));


                     uart_println("\t%s\r\n",q->payload.msg);
                     break;
                  case CONTROL:
	              default:
   	                 uart_println("\tUnknown packet received.\r\n");
   		             break;
               }
               packet_available = 0;
            }
            else if (count==TIME_OUT)
            {
               cState = CONTROLLER;
			   count=0;
               set_standby_mode();
               uart_println("Time out has occurred.\r\n");
            } else
            	count++;
            break;
	     case(CONTROLLER):

			for(i=0; i<5; i++)
			 	_delay_ms(10);

            /* Set "data" being sent to blimp */
            p.type=CONTROL;
            sample_joystick(&X, &Y, &Z);

			p.payload.control.X = (uint8_t)X;
            p.payload.control.Y = (uint8_t)Y;
            p.payload.control.Z = (uint8_t)Z;

			uart_println("\n\rX: %d, Y: %d, Z: %d\n\r", X,Y,Z);

			radio_send(BLIMP_ADDRESS, (uint8_t*) &p);

            radio_set_receive();
			count=0;
            cState=LISTEN;
            break;
         default:
            uart_println("\tState not known.\r\n");
            return 0;
       }


	_delay_ms(POLL_INTERVAL);

	}

    return 0;
}


